using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Windows;
using LegoBotPrima.Actions;
using LegoBotPrimaAbstractions;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;

using W3C.Soap;
using drive = Microsoft.Robotics.Services.Drive.Proxy;

using sonarsensor = Microsoft.Robotics.Services.Sample.Lego.Nxt.SonarSensor.Proxy;
using touchsensor = Microsoft.Robotics.Services.Sample.Lego.Nxt.TouchSensor.Proxy;

using compasssensor = Microsoft.Robotics.Services.Sample.HiTechnic.Compass.Proxy;
using accelerometer = Microsoft.Robotics.Services.Sample.HiTechnic.Accelerometer.Proxy;

using ccrwpf = Microsoft.Ccr.Adapters.Wpf;

namespace LegoBotPrima
{
    [Contract(Contract.Identifier)]
    [DisplayName("LegoBotPrimaService")]
    [Description("LegoBotPrimaService service (no description provided)")]
    internal class LegoBotPrimaService : DsspServiceBase, ILegoBotPrimaService
    {

        private readonly ILegoBotPrimaWorkflow _workflow;

        /// <summary>
        /// Service state
        /// </summary>
        [ServiceState]
        LegoBotPrimaServiceState _state = new LegoBotPrimaServiceState();

        /// <summary>
        /// Main service port
        /// </summary>
        [ServicePort("/LegoBotPrimaService", AllowMultipleInstances = true)]
        LegoBotPrimaServiceOperations _mainPort = new LegoBotPrimaServiceOperations();





        [Partner("NxtDrive", Contract = drive.Contract.Identifier,
                             CreationPolicy = PartnerCreationPolicy.UseExisting)]
        private readonly drive.DriveOperations _drivePort = new drive.DriveOperations();



        [Partner("NxtUltrasonicSensor", Contract = sonarsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        private readonly sonarsensor.UltrasonicSensorOperations _ultrasonicSensorPort = new sonarsensor.UltrasonicSensorOperations();

        [Partner("TouchSensor", Contract = touchsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        private readonly touchsensor.TouchSensorOperations _touchSensorPort = new touchsensor.TouchSensorOperations();

        [Partner("HiTechnicAccelerometerSensor", Contract = accelerometer.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        private readonly accelerometer.AccelerometerOperations _accelerometerSensorPort = new accelerometer.AccelerometerOperations();

        [Partner("HiTechnicCompassSensor", Contract = compasssensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        private readonly compasssensor.CompassSensorOperations _compassSensorPort = new compasssensor.CompassSensorOperations();

        ccrwpf.WpfServicePort _wpfServicePort;

        /// <summary>
        /// Service constructor
        /// </summary>
        public LegoBotPrimaService(DsspServiceCreationPort creationPort) : base(creationPort)
        {

            LegoBotFactory.Get.Container.RegisterInstance(this, LegoBotFactory.Get.Singleton);

            this._workflow = LegoBotFactory.Get.Workflow;
        }

        public ILegoBotPrimaWorkflow Workflow { get { return this._workflow; } }

        protected override void Start()
        {
            ExecuteBotAction(new StopBotAction());

            LogInfo(LogGroups.Console, "Start!");

            this._drivePort.EnableDrive(new drive.EnableDriveRequest(true));

            SpawnIterator(Initialize);

            base.Start();
        }

        protected override void Shutdown()
        {
            base.Shutdown();
            Workflow.Shutdown();
        }



        private IEnumerator<ITask> Initialize()
        {


            #region Create UI
            // create WPF adapter
            this._wpfServicePort = ccrwpf.WpfAdapter.Create(TaskQueue);

            var runWindow = this._wpfServicePort.RunWindow(() => (Window)LegoBotFactory.Get.MainWindow);

            yield return (Choice)runWindow;

            var exception = (Exception)runWindow;
            if (exception != null)
            {
                LogError(exception);
                StartFailed();
                yield break;
            }

            // need double cast because WPF adapter doesn't know about derived window types
            var userInterface = ((Window)runWindow) as IMainPrimaWindow;
            if (userInterface == null)
            {
                var e = new ApplicationException("User interface was expected to be IMainPrimaWindow");
                LogError(e);
                throw e;
            }
            Workflow.UI = userInterface;
            #endregion

            var ultrasonicSensorNotify = new sonarsensor.UltrasonicSensorOperations();

            var subscribe = this._ultrasonicSensorPort.Subscribe(ultrasonicSensorNotify);
            
            yield return (Choice) subscribe;

            var touchSensorNotify = new touchsensor.TouchSensorOperations();

            var subscribe2 = this._touchSensorPort.Subscribe(touchSensorNotify);

            yield return (Choice)subscribe2;

            var accelerometerSensorNotify = new accelerometer.AccelerometerOperations();

            var subscribe3 = this._accelerometerSensorPort.Subscribe(accelerometerSensorNotify);

            yield return (Choice)subscribe3;
            

            var compassSensorNotify = new compasssensor.CompassSensorOperations();

            var subscribe4 = this._compassSensorPort.Subscribe(compassSensorNotify);
            
            yield return (Choice)subscribe4;

            Activate<ITask>(
                Arbiter.Receive<sonarsensor.SonarSensorUpdate>(true, ultrasonicSensorNotify, UltrasonicSensorUpdateHandler),
                Arbiter.Receive<touchsensor.TouchSensorUpdate>(true, touchSensorNotify, TouchSensorUpdateHandler),
                Arbiter.Receive<accelerometer.AccelerometerUpdate>(true, accelerometerSensorNotify, AccelerometerUpdateHandler),
                Arbiter.Receive<compasssensor.CompassSensorUpdate>(true, compassSensorNotify, CompassSensorUpdateHandler)
                );

            
        }

        private void CompassSensorUpdateHandler(compasssensor.CompassSensorUpdate message)
        {
            EnsureUiIsEnabled();
            Workflow.OnCompassChanged(message.Body.Degrees, message.Body.TimeStamp);
        }
        private void TouchSensorUpdateHandler(touchsensor.TouchSensorUpdate message)
        {
            EnsureUiIsEnabled();
            Workflow.OnBumperChanged(message.Body.TouchSensorOn, message.Body.TimeStamp, message.Body.Name);
        }
        private void UltrasonicSensorUpdateHandler(sonarsensor.SonarSensorUpdate message)
        {
            EnsureUiIsEnabled();
            Workflow.OnDistanceChanged(message.Body.Distance, message.Body.TimeStamp);
        }
        private void AccelerometerUpdateHandler(accelerometer.AccelerometerUpdate message)
        {
            EnsureUiIsEnabled();
            Workflow.OnAccelerometerChanged(message.Body.X, message.Body.Y, message.Body.Z, message.Body.TimeStamp);
        }

        private bool _uiIsEnabled;
        private void EnsureUiIsEnabled()
        {
            if(!this._uiIsEnabled)
            {
                this._uiIsEnabled = true;
                Workflow.UI.SetEnabled();
            }
        }

        public void GetDrivePortState(Handler<drive.DriveDifferentialTwoWheelState> success, Handler<Fault> error)
        {
            Activate(
                Arbiter.Choice(
                        this._drivePort.Get(),
                        success,
                        error
                    )
                );
        }

        public void SetDrivePower(double leftWheelPower, double rightWheelPower)
        {
            //this._drivePort.SetDrivePower(new drive.SetDrivePowerRequest(leftWheelPower, rightWheelPower));
            Activate(
                    Arbiter.Choice(
                        this._drivePort.SetDrivePower(new drive.SetDrivePowerRequest(leftWheelPower, rightWheelPower)),
                        r => { },
                        f => Workflow.Error("Error! Connot Set Drive Power", f))
                    );
        }

        public void ExecuteBotAction(IBotAction action)
        {
            LogInfo(LogGroups.Console, "BotAction:" + action);

            Workflow.SetAction(action);
        }

        




    }
}


